#pragma once
#include "Matrix.h"
#include "Vector3.h"
using namespace NBE;
namespace banknamespace
{
#define G (1/360.0f)
	class Physics
	{
	public:
		Physics()
			:m_prev(state()),m_next(state())
		{}
		~Physics(){}
		struct state
		{
			Matrix3f R;
			vec3f Hi;//angular momentum
			vec3f Vcm;
			//vec3f Vi;
			//vec3f ri;// c.m -> pos i
			vec3f Rcm;//world basis
			////////////////////////////////////////////////////////////ap//////////////
			Matrix3f Irb;//rb space inertia tensor
			Matrix3f I_inv;//inertia tensor inverse matrix
			vec3f w;//angular velocity
			vec3f ForceDir;//world basis
			float Mass;
			//////////////////////////////////////////////////////////////////////////

		public:
			state(float _Mass = 1):
			R(Matrix3f::Identity()),Hi(vec3f()),Rcm(vec3f()),Irb(Matrix3f::Identity()),
				ForceDir(vec3f()),Mass(_Mass),w(vec3f()),I_inv(Matrix3f::Identity())
			{}
			Matrix3f RotateAngleAxis(vec3f axis,float angle)
			{
				//rotate the body, and return the R
				float cosAngle = cos(angle);
				float sinAngle = sin(angle);
				float one_cos = 1 - cosAngle;
				Matrix3f result = 
					Matrix3f(cosAngle + axis.x * axis.x * one_cos , axis.x * axis.y * one_cos - axis.z *sinAngle, axis.x * axis.z * one_cos + axis.y *sinAngle,
							 axis.y * axis.z * one_cos + axis.z *sinAngle, cosAngle + axis.y * axis.y * one_cos , axis.y * axis.z * one_cos - axis.x *sinAngle,
							 axis.z * axis.x * one_cos - axis.y *sinAngle, axis.z * axis.y * one_cos + axis.x *sinAngle, cosAngle + axis.z * axis.z * one_cos
					);



				return    result;
			}

	 
		};
		
		Matrix3f getRotationDerivative(vec3f w, Matrix3f rot)
		{
// 			if (w.dis(vec3f()) == 0)
// 			{
// 				return rot;
// 			}
			Matrix3f new_rot;
			vec3f iB(rot[0],rot[3],rot[6]); 
			vec3f jB(rot[1],rot[4],rot[7]); 
			vec3f kB(rot[2],rot[5],rot[8]); 
			vec3f col1 = w.crossProduct(iB);
			vec3f col2 = w.crossProduct(jB);
			vec3f col3 = w.crossProduct(kB);
			new_rot = Matrix3f(col1[0],col2[0],col3[0],
				col1[1],col2[1],col3[1],
				col1[2],col2[2],col3[2]);
			return new_rot;
		}

		state m_prev;
		state m_next;
	private:
		
 

		
	};
}